/************************************************************
 * @file: motor_commutation.c
 * @author: Hao
 * @version: V0.0
 * @data: 2023/07/28
 * @brief: novosns controller lib
 * @note:
 * @Copyright (C) 2023 Novosense All rights reserved.
 ************************************************************/

/************************************************************
 ************************************************************
 * @par Edition History
 * -V0.0  2023.07.28
 *        - Initilization file
 *
 ************************************************************/
 
#include "nvsns_controller.h"

/************************************************************
 * @brief: PID Vars init
 * @return <None>
 ************************************************************/
void PID_IncrementalInit(PidIncremental_t *tPid) 
{
    // Initialize variables
    tPid->Errn  = 0;
    tPid->Errn1 = 0;
    tPid->Out   = 0;
    tPid->Out1  = 0;

    // Initialize coefficients
    tPid->B2 = tPid->Kd;
    tPid->B1 = -SatAddF16(tPid->Kp,MulF16(tPid->Kd,2));
    tPid->B0 = SatAddF16(tPid->Kp,tPid->Ki);
}

/************************************************************
 * @brief: PID calculate
 * @return <None>
 ************************************************************/
void PID_IncrementalUpdate(PidIncremental_t *tPid) 
{
    /* Calculate error */
//	tPid->Errn = (tPid->Ref - tPid->Fdbk);
    
    #if DEBUG_PARAMETER_ENABLE
    tPid->B2 = tPid->Kd;
    tPid->B1 = -SatAddF16(tPid->Kp,MulF16(tPid->Kd,2));
    tPid->B0 = SatAddF16(tPid->Kp,tPid->Ki);
    #endif
    
    tPid->Out = SatAddF16(tPid->Out1,MulF16(tPid->B0,tPid->Errn));
    tPid->Out = SatAddF16(tPid->Out,MulF16(tPid->B1,tPid->Errn1));
    tPid->Out = SatAddF16(tPid->Out,MulF16(tPid->B2,tPid->Errn2));
    
    /* Update error value */
    tPid->Errn2 = tPid->Errn1;
    tPid->Errn1 = tPid->Errn;
    
    /* Saturated output */
    tPid->Out = (tPid->Out < tPid->Max) ? tPid->Out : tPid->Max;
    tPid->Out = (tPid->Out > tPid->Min) ? tPid->Out : tPid->Min;

    /* Store output */
    tPid->Out1 = tPid->Out;
}

/************************************************************
 * @brief: PID Vars init
 * @return <None>
 ************************************************************/
void PID_PidPositionalInit(PidPositional_t *tPid) 
{
    // Initialize variables
    tPid->Inter = 0;
    tPid->Errn  = 0;
    tPid->Errn1 = 0;
    tPid->Out   = 0;
}

/************************************************************
 * @brief: PID calculate
 * @return <None>
 ************************************************************/
void PID_PositionalUpdate(PidPositional_t *tPid) 
{
    /* Calculate error */
    tPid->Errn = tPid->Ref - tPid->Fdbk;
    
    tPid->Inter = SatAddF16(tPid->Inter, MulF16(tPid->Ki, tPid->Errn));
    tPid->Inter = (tPid->Inter < tPid->InterMax) ? tPid->Inter : tPid->InterMax;
    tPid->Inter = (tPid->Inter > tPid->InterMin) ? tPid->Inter : tPid->InterMin;
    
    tPid->Out = MulF16(tPid->Kp, tPid->Errn);
    tPid->Out = SatAddF16(tPid->Out, tPid->Inter);
    tPid->Out = SatAddF16(tPid->Out, MulF16(tPid->Kd, SatSubF16(tPid->Errn, tPid->Errn1)));

    /* Update error value */
    tPid->Errn1 = tPid->Errn;
    
    /* Saturated output */
    tPid->Out = (tPid->Out < tPid->Max) ? tPid->Out : tPid->Max;
    tPid->Out = (tPid->Out > tPid->Min) ? tPid->Out : tPid->Min;
}

